﻿#include "FFT.h"
#include <iostream>
#include "lang/System.h"
#include "lang/Exception.h"
#include "array/Array.hpp"
#include "array/Arrays.h"
using namespace yzrilyzr_lang;
using namespace yzrilyzr_array;
namespace yzrilyzr_dsp{
	FFT::FFT(u_index fftLength) :
		fftLength(fftLength),
		sinTable(fftLength/2),
		cosTable(fftLength/2),
		cpRe(fftLength),
		cpIm(fftLength){
		computeM();
		generateSinCosTable();
		generateComplexArray();
	}
	FFT::~FFT(){

	}
	void FFT::ifft(){
		for(int32_t i=0;i < fftLength;i++){
			cpIm[i]=-cpIm[i];
		}
		fft();
		for(int32_t i=0;i < fftLength;i++){
			cpRe[i]/=(u_sample)fftLength;
			cpIm[i]/=(u_sample)fftLength;
		}
	}
	void FFT::generateComplexArray(){
		Arrays::fill(cpIm, static_cast<u_sample>(0.0));
		Arrays::fill(cpRe, static_cast<u_sample>(0.0));
	}
	/*void FFT::multiple(Complex ** a, Complex ** b, Complex ** output, int32_t complexLength){
		for(int32_t i=0;i < complexLength;i++){
			Complex * out=output[i];
			Complex * aa=a[i];
			Complex * bb=b[i];
			out->re=aa->re * bb->re - aa->im * bb->im;
			out->im=aa->re * bb->im + aa->im * bb->re;
		}
	}*/
	void FFT::computeM(){
		if((fftLength & (fftLength - 1)) != 0){
			throw Exception("FFT length must be a power of 2.");
		}
		u_index length=fftLength;
		M=0;
		while((length & 0b1) == 0){
			M+=1;
			length>>=1;
		}
	}
	void FFT::generateSinCosTable(){
		for(int32_t i=0;i < fftLength / 2;i++){
			u_sample x= Math::TAU * i / fftLength;
			sinTable[i]=-sin(x);
			cosTable[i]=cos(x);
		}
	}
	void FFT::fft(){
		u_index hLength=fftLength >> 1;
		u_index j=0;
		for(u_index i=0; i < fftLength; ++i){
			if(i < j){
				std::swap(cpRe[i], cpRe[j]);
				std::swap(cpIm[i], cpIm[j]);
			}
			u_index m=hLength;
			while(j >= m && m > 0){
				j-=m;
				m>>=1;
			}
			j+=m;
		}
		int32_t ff=0;
		u_index complexLengthM1=fftLength - 1;
		for(int32_t m=1;m <= M;m++){
			int32_t la=1 << m;//2^m
			int32_t lb=la >> 1;//2^(m-1)
			int32_t mask=(1 << (M - m));
			for(int32_t l=0;l < lb;l++){
				int32_t r=l * mask;
				u_sample slr=sinTable[r];
				u_sample clr=cosTable[r];
				for(int32_t n=l;n < complexLengthM1;n+=la){
					u_index index2=n + lb;
					u_sample c2Re=cpRe[index2];
					u_sample c2Im=cpIm[index2];
					u_sample c1Re=cpRe[n];
					u_sample c1Im=cpIm[n];
					u_sample re=c2Re * clr - c2Im * slr;
					u_sample im=c2Re * slr + c2Im * clr;
					cpRe[index2]=c1Re - re;
					cpIm[index2]=c1Im - im;
					cpRe[n]=c1Re + re;
					cpIm[n]=c1Im + im;
				}
			}
		}
	}
	void FFT::setComplex(u_index index, u_sample real, u_sample imag){
		cpRe[index]=real;
		cpIm[index]=imag;
	}
	/*Complex& FFT::getComplex(u_index index){
		return *complexes[index];
	}*/
	/*void FFT::generateComplexArray(){
		complexes=new Complex * [fftLength];
		for(int32_t i=0;i < fftLength;i++){
			complexes[i]=new Complex();
		}
	}*/
	void FFT::input(SampleArray & buffer){
		Arrays::fill(cpIm, static_cast<u_sample>(0));
		System::arraycopy(buffer, 0, cpRe, 0, fftLength);
	}

	void FFT::outputMagnitude(SampleArray & buffer){
		for(int32_t i=0,j=fftLength;i < j;i++){
			buffer[i]=magnitudeAt(i);
		}
	}
	
	void FFT::outputPhase(SampleArray & buffer){
		for(int32_t i=0, j=fftLength;i < j;i++){
			buffer[i]=phaseAt(i);
		}
	}
	void FFT::inputRingBuffer(RingBufferSample * buffer, int32_t length){
		Arrays::fill(cpIm, static_cast<u_sample>(0));
		for(int32_t i=0;i < length;i++){
			cpRe[i]=buffer->getFromOldest(i);
		}
	}
	u_sample FFT::phaseAt(u_index index) const{
		return std::atan2(cpIm[index], cpRe[index]);
	}
	u_sample FFT::magnitudeAt(u_index index) const{
		return std::hypot(cpRe[index], cpIm[index]) * 2.0 / static_cast<u_sample>(fftLength);
	}
	u_sample FFT::realAt(u_index index) const{
		return cpRe[index];
	}
	u_sample FFT::imagAt(u_index index) const{
		return cpIm[index];
	}
}